/**
 * @file pid.c
 * @author Yifan Xiao (1993996310@qq.com)
 * @brief Ref: https://zhuanlan.zhihu.com/p/373402745
 * @version 0.1
 * @date 2022-01-25
 *
 * @copyright Copyright (c) 2022
 *
 */

#include "rtthread.h"
#include "pid.h"
#include "math.h"


/**
 * @brief PID 算法
 *
 * @param data
 * @param error
 */
void ist_pid_handle(PID_Data *data, double error)
{
    double err = error;
    double err_diff = err - data->pre_err1;
    data->pre_err1 = err;
    data->integral += err;
    data->output = data->kp * err + data->ki * data->integral + data->kd * err_diff;
    if (data->output > data->max_output)
    {
        data->output = data->max_output;
    }
    else if (data->output < -data->max_output)
    {
        data->output = -data->max_output;
    }
}

void ist_pid_create(PID_Data *data, double kp, double ki, double kd)
{
    data->kp = kp;
    data->ki = ki;
    data->kd = kd;
}

void ist_pid_setmax(PID_Data *data, double value)
{
    data->max_output = value;
}

void ist_pid_fix(PID_Data *data, uint8_t k_type, double value)
{
    switch (k_type)
    {
    case K_TYPE_P:
        data->kp = value;
        break;

    case K_TYPE_I:
        data->ki = value;
        break;   

    case K_TYPE_D:
        data->kd = value;
        break;
        
    default:
        break;
    }
}
